#include "physicsServer.h"
#include "eventServer.h"

using namespace sde;

PhysicsServer::PhysicsServer(std::string _type, std::string _name, Object* _object) :
    Action(_type,_name,_object),
    broadphase(0),
    collisionConfiguration(0),
    dispatcher(0),
    solver(0),
    dynamicsWorld(0),
    frameCount(0) {

    int maxProxies = 1024;
    btVector3 worldAabbMin(-10000,-10000,-10000);
    btVector3 worldAabbMax(10000,10000,10000);
    broadphase = new btAxisSweep3(worldAabbMin,worldAabbMax,maxProxies);

    collisionConfiguration = new btDefaultCollisionConfiguration();
    dispatcher = new btCollisionDispatcher(collisionConfiguration);

    solver = new btSequentialImpulseConstraintSolver;
    
    dynamicsWorld = new btDiscreteDynamicsWorld(dispatcher,broadphase,solver,collisionConfiguration);
    
    EventServer::getSingleton().subscribe("update",boost::bind(&PhysicsServer::update,this,_1));
}

PhysicsServer::~PhysicsServer() {
    if (dynamicsWorld) {
        delete broadphase;
        delete collisionConfiguration;
        delete dispatcher;
        delete solver;
        delete dynamicsWorld;
    }
}

void PhysicsServer::update(VarMap args) {
    if (dynamicsWorld) {
        lastTime = boost::any_cast<float>(args["time"]);
        if (!frameCount++) startTime = lastTime;
    
        float dtime = boost::any_cast<float>(args["dtime"]);
        if (dtime > 0) {
            dynamicsWorld->stepSimulation(dtime,10);
        }
    }
}

void PhysicsServer::stop() {
    log("physics fps: %f",float(frameCount) / (lastTime - startTime));
}

btVector3 PhysicsServer::VectorToBulletVector(Vector3 v) {
    return btVector3(v.x,v.y,v.z);
}

Vector3 PhysicsServer::BulletVectorToVector(btVector3 v) {
    return Vector3(v.x(),v.y(),v.z());
}

btQuaternion PhysicsServer::QuaternionToBulletQuaternion(Quaternion q) {
    return btQuaternion(q.x,q.y,q.z,q.w);
}

Quaternion PhysicsServer::BulletQuaternionToQuaternion(btQuaternion q) {
    return Quaternion(q.w(),q.x(),q.y(),q.z());
}
